机器人力控混合拖动功能包调用介绍文档

[TOC]

本文档介绍机器人力控混合拖动功能包的当前调用流程。内容以 aral_export/include/aral/robot_featpack.hpp 的头文件定义和 src/robot_featpack_impl.cpp 的实现为准。

当前功能包接口为：

int FPHandGuidingEnable(const LoggerPtr& log,
                        const interface::StatePtr& state,
                        const interface::ControllerPtr& control,
                        const HandGudingEnable& enable,
                        HandGuidingContext& ctx);

int FPHandGuidingUpdate(const interface::StatePtr& state,
                        const interface::ControllerPtr& control,
                        const HandGudingUpdate& update,
                        interface::JointCommand& out,
                        const HandGuidingContext& ctx);

ctx 由 FPHandGuidingEnable 填充，后续每次 FPHandGuidingUpdate 必须传入同一个上下文，用于奇异通过时的控制模式切换。

1 支持的拖动类型

FPHandGuidingEnable 根据 direction、command、space、singular、sensor 选择拖动分支。

1.1 六轴方向全开关节阻抗力矩拖动

关键参数：

enable.direction = {1, 1, 1, 1, 1, 1};
enable.command = ARAL::pack::CommandType::TORQUE;
enable.space = ARAL::interface::DescribeSpace::JOINT;
enable.singular = ARAL::pack::SingularProtectType::UNDEFINED;

实现分支：SetJointImpParaForHandGuiding，内部控制器为关节阻抗。

主要参数：

• jnt_damp：传入 fcSetDampPercent，作为关节侧阻尼百分比。
• control_period：传入 mcSetControlPeriod。
• force_threshold：传入 fcSetForceThreshold。
• force_limit：传入 fcSetForceLimit。

输出使用：读取 out.torq。

1.2 六轴方向全开笛卡尔阻抗力矩拖动

关键参数：

enable.direction = {1, 1, 1, 1, 1, 1};
enable.command = ARAL::pack::CommandType::TORQUE;
enable.space = ARAL::interface::DescribeSpace::CARTESIAN;
enable.singular = ARAL::pack::SingularProtectType::UNDEFINED;

实现分支：SetCartesianImpParaForHandGuiding，内部控制器为笛卡尔阻抗。

主要参数：

• jnt_damp：传入 fcSetDampPercent，作为关节侧阻尼百分比。
• control_period：传入 mcSetControlPeriod。
• end_damp：当 sensor == FTSensorType::END 时用于计算笛卡尔阻抗惯量系数；当 sensor == FTSensorType::JOINT 时用于计算笛卡尔阻尼。
• force_threshold：传入 fcSetForceThreshold。
• force_limit：传入 fcSetForceLimit。

输出使用：读取 out.torq。

1.3 六轴方向全开位置拖动，不过奇异

关键参数：

enable.direction = {1, 1, 1, 1, 1, 1};
enable.command = ARAL::pack::CommandType::POSITION;
enable.singular = ARAL::pack::SingularProtectType::UNDEFINED;

实现分支：SetJointAdmParaForHandGuiding，内部控制器为关节导纳。

主要参数：

• jnt_damp：用于计算关节导纳质量，值越大质量越大，手感越沉。
• control_period：传入 mcSetControlPeriod。
• force_threshold：关节力响应阈值。
• force_limit：关节力响应极限。

输出使用：读取 out.q。

1.4 六轴方向全开位置拖动，可过奇异

关键参数：

enable.direction = {1, 1, 1, 1, 1, 1};
enable.command = ARAL::pack::CommandType::POSITION;
enable.singular = ARAL::pack::SingularProtectType::PASS;

实现分支：

• Enable 阶段先预设置一组固定关节导纳参数，用于奇异区保护。
• Enable 阶段随后调用 SetCartesianAdmParaForHandGuiding，正常区域使用笛卡尔导纳。
• Update 阶段根据 state->rsGetSingularityProperty() 动态在笛卡尔导纳和关节导纳之间切换。

主要参数：

• end_damp：正常区域笛卡尔导纳手感，用于计算笛卡尔质量和阻尼。
• control_period：传入 mcSetControlPeriod。
• 奇异区关节导纳参数由实现内部固定值设置，不使用 jnt_damp。
• force_threshold：笛卡尔力响应阈值。
• force_limit：笛卡尔力响应极限。

输出使用：读取 out.q。

1.5 非六轴方向拖动

关键参数示例：

enable.direction = {1, 0, 0, 0, 0, 0};
enable.command = ARAL::pack::CommandType::POSITION;
enable.singular = ARAL::pack::SingularProtectType::UNDEFINED;

实现分支：非六轴方向全开时不检查 command 和 space，直接进入笛卡尔导纳分支。若 singular == PASS，会先预设置奇异区关节导纳参数。

主要参数：

• end_damp：非六轴方向笛卡尔导纳手感，用于计算笛卡尔质量和阻尼。
• control_period：传入 mcSetControlPeriod。
• direction：传入笛卡尔选择向量，决定开放的拖动方向。
• task_frame：Update 阶段用于设置目标力所在任务坐标系。BASE 坐标系拖动可设置为全 0；TOOL/TCP 坐标系拖动应每周期更新为当前 TCP 位姿。
• force_threshold：笛卡尔力响应阈值。
• force_limit：笛卡尔力响应极限。

输出使用：读取 out.q。

2 使能结构体参数说明

当前 robot_featpack.hpp 中的使能结构体为：

struct HandGudingEnable
{
    RLJntArray               q, qd, qdd;
    ToolWorkpiece            t_w;
    std::vector<int>         direction;
    CommandType              command{-1};
    SingularProtectType      singular{-1};
    FTSensorType             sensor{-1};
    DescribeSpace            space{-1};
    RLJntArray               jnt_damp;
    DoubleVec                end_damp;
    DoubleVec                force_threshold;
    DoubleVec                force_limit;
    double                   control_period{ARAL_CONTROL_PERIOD};
};

参数说明：

• q、qd、qdd：拖动开始时的关节实际位置、速度、加速度，用于初始化机器人状态。
• t_w：工具工件信息，用于状态初始化。
• direction：拖动方向选择向量，长度必须为 6。全 1 表示六轴方向全开；非全 1 表示指定方向拖动。
• command：关节指令类型。TORQUE 输出力矩指令；POSITION 输出位置指令。六轴方向全开时该字段先决定进入阻抗或导纳分支。
• singular：奇异防护类型。UNDEFINED 表示不启用奇异通过；PASS 表示允许奇异区切换到关节导纳；STOP 当前没有单独停止分支，代码会按非 UNDEFINED 进入笛卡尔导纳分支，但不会设置 ctx.singular_pass。
• sensor：传感器类型。仅笛卡尔阻抗分支中用于区分无传感器、末端传感器和关节传感器处理。
• space：空间类型。仅六轴方向全开且 command == TORQUE 时使用：JOINT 进入关节阻抗拖动，CARTESIAN 进入笛卡尔阻抗拖动；其他值返回 E_FP_FUNC_NOT_IMPL。
• jnt_damp：关节手感参数，长度为机器人自由度，建议 [0, 1]。关节导纳分支用于计算关节质量；笛卡尔阻抗分支作为关节阻尼百分比。
• end_damp：末端手感参数，长度为 6，建议 [0, 1]。笛卡尔导纳分支用于计算笛卡尔质量和阻尼；笛卡尔阻抗带传感器分支用于计算惯量系数或阻尼。
• force_threshold：力响应阈值，长度为 6。
• force_limit：力响应极限，长度为 6。
• control_period：控制周期。当前关节导纳、笛卡尔导纳、笛卡尔阻抗、关节阻抗分支都会传入 mcSetControlPeriod。

当前 HandGudingEnable 中没有 q_init、qd_init、qdd_init、tool_workpiece、direciton、cmd_type、singular_protect、end_omnidirectional_damp、end_directional_damp 字段。

3 更新结构体参数说明

当前 robot_featpack.hpp 中的更新结构体为：

struct HandGudingUpdate
{
    RobotState                  state;
    interface::RLPose           task_frame;
    double                      filt_fre{0};
    HandGuidingContext          ctx;
};

RobotState 字段为：

struct RobotState
{
    RLJntArray      q;
    RLJntArray      qd;
    RLJntArray      qdd;
    RLJntArray      torque;
    RLJntArray      temperature;
    RLJntArray      friction;
    RLWrench        sensor{};
};

参数说明：

• state.q、state.qd、state.qdd：实时关节位置、速度、加速度。
• state.torque、state.temperature、state.friction：实时关节力矩、温度、摩擦力矩。Update 中通过 rsUpdateJointState(..., update.filt_fre) 更新。
• state.sensor：末端传感器数据，建议由外部完成零偏扣除。Update 中固定按 FTSensorType::END 写入。
• task_frame：仅当前控制类型为笛卡尔导纳时用于 fcSetGoalForce 的任务坐标系；关节导纳/笛卡尔阻抗时不会作为目标力坐标系使用。
• filt_fre：状态更新时的滤波截止频率，默认 0 表示不滤波。
• ctx：结构体内保留上下文字段；当前实现实际使用函数入参 const HandGuidingContext& ctx 进行奇异切换判断。

当前 HandGudingUpdate 中没有 space、sensor_type、command、direction 字段。

4 典型调用流程

SUITE_ARAL_RC_FUNCTION_HAND_GUIDING 中 rcTestHandGuidingWorkflowFeatPack 覆盖以下 8 类流程。当前 test/data/control/hand_guiding/pack/*.toml 已包含 space 配置项，HandGuidingFeatPackTest 会从 TOML 读取并写入 enable.space；全方向力矩拖动用例需配置 space = 0 或 space = 1，分别进入关节阻抗或笛卡尔阻抗分支。

典型调用顺序参考 HandGuidingFeatPackTest。测试中先通过 SetRobotModelAndGenerateTestData 设置模型参数并加载回放数据，再构造 HandGudingEnable，最后循环调用 FPHandGuidingUpdate。

int ret = E_NO_ERROR;

// 1. 按当前机器人型号创建/初始化 model、state、controller。
// 测试中由 AuboRobotScene::Setup(robot_type) 完成。

// 2. 设置模型参数。以下参数来自 test/data/control/hand_guiding/pack/*.toml 的 [Model] 段。
interface::Array3d gravity_acc = {0.0, 0.0, -9.81};
interface::DoubleVec dynamic_para = link_dynamic_para;
interface::RLJntArray torq_constant = joint_torque_constant;
interface::RLJntArray jnt_pos_upper = joint_upper_limit;
interface::RLJntArray jnt_pos_lowwer = joint_lower_limit;
interface::RLPose sensor_pose = sensor_pose_in_flange;
interface::RLPose tcp_pose = tcp_pose_in_flange;

interface::RLInertia payload_para;
payload_para.mass = payload_mass;
payload_para.center = payload_center;
payload_para.inertia.fill(0);

std::vector<interface::FrictionParam> friction_para(model->mdlGetRobotDOF());
for (unsigned int i = 0; i < model->mdlGetRobotDOF(); ++i) {
    friction_para[i].type = interface::FrictionType::VTL_Model;
    friction_para[i].para = {
        Fs[i], Fc[i], Vs[i], Miu[i],
        Fv0[i], Fv1[i], Fv2[i], Fv3[i],
        Ft1[i], Ft2[i], Ft3[i],
        c1[i], c2[i]
    };
}

ret = model->mdlSetGravityVectorInBase(gravity_acc);
if (ret < 0) return ret;
ret = model->mdlSetJointTorqueConstant(torq_constant);
if (ret < 0) return ret;
ret = model->mdlSetRobotLinkDynamicParameter(dynamic_para);
if (ret < 0) return ret;
ret = model->mdlSetLoadDynamicParameterInFlange(payload_para);
if (ret < 0) return ret;
ret = model->mdlSetJointFrictionParameter(friction_para);
if (ret < 0) return ret;
ret = model->mdlSetJointPositionRange(jnt_pos_upper, jnt_pos_lowwer);
if (ret < 0) return ret;
ret = model->mdlSetEndSensorPoseInFlange(sensor_pose);
if (ret < 0) return ret;
ret = model->mdlSetToolPose(tcp_pose);
if (ret < 0) return ret;

// 3. 加载或采集拖动周期数据。测试中从 [Data].path 指向的 CSV 读取：
// actual_q/actual_qd/actual_qdd、actual_torque、actual_temperature、
// actual_friction、sensor_data、task_frame，以及期望输出 cmd_q/cmd_torque。

ARAL::pack::HandGudingEnable enable;
ARAL::pack::HandGuidingContext ctx;
ARAL::pack::HandGudingUpdate update;
ARAL::interface::JointCommand out;

// 4. 使用首个周期的真实状态初始化拖动使能结构体。
enable.q = q0;
enable.qd = qd0;
enable.qdd = qdd0;
enable.t_w = interface::ToolWorkpiece();
enable.direction = {1, 1, 1, 1, 1, 1};
enable.command = ARAL::pack::CommandType::TORQUE;
enable.space = ARAL::interface::DescribeSpace::CARTESIAN;
enable.singular = ARAL::pack::SingularProtectType::UNDEFINED;
enable.sensor = ARAL::interface::FTSensorType::END;
enable.jnt_damp = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
enable.end_damp = {0.9, 0.9, 0.9, 0.6, 0.6, 0.6};
enable.force_threshold = {1.0, 1.0, 1.0, 0.2, 0.2, 0.2};
enable.force_limit = {100.0, 100.0, 100.0, 10.0, 10.0, 10.0};

ret = ARAL::pack::FPHandGuidingEnable(log, state, controller, enable, ctx);
if (ret < 0) {
    return ret;
}

// 5. 控制周期内持续更新真实状态和传感器数据，计算关节输出。
while (running) {
    update.ctx = ctx;
    update.state.q = q;
    update.state.qd = qd;
    update.state.qdd = qdd;
    update.state.sensor = sensor_wrench_without_offset;
    update.state.torque = torque;
    update.state.friction = friction;
    update.state.temperature = temperature;
    update.task_frame = current_task_frame;
    update.filt_fre = 0;

    ret = ARAL::pack::FPHandGuidingUpdate(state, controller, update, out, ctx);
    if (ret < 0) {
        break;
    }

    // command == TORQUE 时读取 out.torq；
    // command == POSITION 时读取 out.q。
}

模型参数设置顺序与测试保持一致：mdlSetGravityVectorInBase、mdlSetJointTorqueConstant、mdlSetRobotLinkDynamicParameter、mdlSetLoadDynamicParameterInFlange、mdlSetJointFrictionParameter、mdlSetJointPositionRange、mdlSetEndSensorPoseInFlange、mdlSetToolPose。其中摩擦参数使用 FrictionType::VTL_Model，每个关节 13 个参数顺序为 Fs/Fc/Vs/Miu/Fv0/Fv1/Fv2/Fv3/Ft1/Ft2/Ft3/c1/c2。

5 常用参数组合

6 注意事项

• direction.size() 必须等于 6，否则 FPHandGuidingEnable 返回 E_FP_INPUT_ERROR。
• 六轴方向全开时必须设置 command 为 TORQUE 或 POSITION；否则返回 E_FP_FUNC_NOT_IMPL。
• 六轴方向全开且 command == TORQUE 时必须设置 space 为 JOINT 或 CARTESIAN；否则返回 E_FP_FUNC_NOT_IMPL。
• singular == PASS 时，必须把 FPHandGuidingEnable 输出的 ctx 传给每次 FPHandGuidingUpdate。
• update.ctx 字段当前也应同步赋值为同一个 ctx，便于后续接口实现调整。
• state.sensor 每周期按末端传感器数据更新；若没有末端传感器，也应保证数组有效并按工程约定填 0。
• force_threshold 不宜过小，否则噪声会触发拖动响应；force_limit 不宜过大，否则异常外力可能导致输出过大。